/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package utilities;

import com.googlecode.javacv.FrameGrabber.Exception;
import java.util.logging.Level;
import java.util.logging.Logger;
import static com.googlecode.javacv.cpp.opencv_core.*;
import static com.googlecode.javacv.cpp.opencv_imgproc.*;
import com.googlecode.javacv.*;
import com.googlecode.javacv.cpp.opencv_calib3d;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import java.awt.event.KeyEvent;
import java.util.Vector;

/**
 *
 * @author Shanika
 */
public class CameraCalibration {

    private int numSquares, successes = 0, found;
    private int[] corner_count;
    private CvMat object_point, image_point, point_count, intrinsic, distCoeffs;
    private IplImage image, gray_image, imageUndistorted;
    private FrameGrabber grabber;
    private CvSize board_sz;
    private CvPoint2D32f corners;
    private KeyEvent ke;
    public static Vector<CvMat> camMatrix = new Vector<CvMat>();
    public static Vector<CvMat> camDistCoef = new Vector<CvMat>();

//    public static void main(String[] args) {
//        CameraCalibration cc=new CameraCalibration();
//
//    }
    public void calculateCameraMatrix(int numBoards, int numCornersHor, int numCornersVer, int cameraNo) {


        CanvasFrame canvas = new CanvasFrame("Original Image");
        CanvasFrame canvas1 = new CanvasFrame("Gray Image");
        grabber = new VideoInputFrameGrabber(cameraNo);
        try {
            grabber.start();

            numSquares = numCornersHor * numCornersVer;
            object_point = cvCreateMat(numSquares * numBoards, 3, CV_32FC1);// Physical points in 3D space
            image_point = cvCreateMat(numSquares * numBoards, 2, CV_32FC1);// Image points in 2D space
            board_sz = new CvSize(numCornersHor, numCornersVer); // Size of the board
            corner_count = new int[numSquares];
            point_count = cvCreateMat(numBoards, 1, CV_32SC1);

// Initializing image_point and object_point
            for (int i = 0; i < numSquares; i++) {
                image_point.put(i, 0, 0.0);
                image_point.put(i, 1, 0.0);

                object_point.put(i, 0, i / numCornersHor);
                object_point.put(i, 1, i % numCornersHor);
                object_point.put(i, 2, 0);
            }
            for (int i = 0; i < numBoards; i++) {
                point_count.put(i, 0, 0.0); // Initializing point_count
            }

            while (successes < numBoards) {
                corners = new CvPoint2D32f(numSquares);
                try {
                    image = grabber.grab(); // Capturing Images of the chessboard
                } catch (Exception ex) {
                    System.out.println(ex);
                }
                gray_image = cvCreateImage(cvGetSize(image), 8, 1);
                cvCvtColor(image, gray_image, CV_BGR2GRAY); // Convert image to gray
                canvas.showImage(image);
                canvas1.showImage(gray_image);

// Finding chessboard corners
                found = opencv_calib3d.cvFindChessboardCorners(image, board_sz, corners, corner_count, opencv_calib3d.CV_CALIB_CB_ADAPTIVE_THRESH | opencv_calib3d.CV_CALIB_CB_FILTER_QUADS);
                System.out.println("found= " + found);

                if (found != 0) {
                    cvFindCornerSubPix(gray_image, corners, corner_count[0], cvSize(11, 11), cvSize(-1, -1),
                            cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); // Finding sub corners
                    opencv_calib3d.cvDrawChessboardCorners(gray_image, board_sz, corners, corner_count[0], found); // Draw corners
                }

                canvas1.showImage(gray_image);
                try {
                    ke = canvas.waitKey(0);
                } catch (InterruptedException ex) {
                    System.out.println(ex);
                }
                int key = ke.getKeyCode();

                if (key == ' ' && found != 0) // Space Bar to save point
                {
                    int step = numSquares * successes;
                    //Saving points to Matrix
                    for (int i = step, j = 0; j < numSquares; i++, j++) {
                        image_point.put(i, 0, corners.position(j).x());
                        image_point.put(i, 1, corners.position(j).y());

                        object_point.put(i, 0, j / numCornersHor);
                        object_point.put(i, 1, j % numCornersHor);
                        object_point.put(i, 2, 0);
                    }

                    point_count.put(successes, 0, corner_count[0]);
                    System.out.println("Snap stored!");
                    successes++;
                }
            }
            intrinsic = cvCreateMat(3, 3, CV_32FC1);
            distCoeffs = cvCreateMat(5, 1, CV_32FC1);

            intrinsic.put(0, 0, 1);
            intrinsic.put(1, 1, 1);
            CvMat rvecs = cvCreateMat(numBoards, 3, CV_32FC1);
            CvMat tvecs = cvCreateMat(numBoards, 3, CV_32FC1);

            opencv_calib3d.cvCalibrateCamera2(object_point, image_point, point_count, image.cvSize(), intrinsic, distCoeffs,
                    tvecs, rvecs, opencv_calib3d.CV_CALIB_ZERO_TANGENT_DIST);

            camMatrix.add(cameraNo, intrinsic);
            camDistCoef.add(cameraNo, distCoeffs);
            
            canvas.dispose();
            canvas1.dispose();
            grabber.stop();

        } catch (Exception ex) {
            Logger.getLogger(CameraCalibration.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    public IplImage imageReconstruction(IplImage orgImage, int cameraNo) {

        imageUndistorted = cvCreateImage(cvGetSize(orgImage), orgImage.depth(), orgImage.nChannels());
        CvMat camIntrinsic = camMatrix.get(cameraNo);
        CvMat camDist = camDistCoef.get(cameraNo);
        cvUndistort2(orgImage, imageUndistorted, camIntrinsic, camDist);
        return imageUndistorted;
    }
}
